//
// Created by xiang on 22-12-6.
//

#include "frontend.h"
#include "ch8/lio-iekf/lio_iekf.h"
#include "common/io_utils.h"

#include <yaml-cpp/yaml.h>

namespace sad {
    
DEFINE_double(antenna_angle, 0.0, "RTK天线安装偏角（角度）");
DEFINE_double(antenna_pox_x, -0.25, "RTK天线安装偏移X");
DEFINE_double(antenna_pox_y, 0.0, "RTK天线安装偏移Y");

Frontend::Frontend(const std::string& config_yaml) { config_yaml_ = config_yaml; }

bool Frontend::Init() {
    LOG(INFO) << "load yaml from " << config_yaml_;
    auto yaml = YAML::LoadFile(config_yaml_);
    try {
        auto n = yaml["data_path"];
        LOG(INFO) << Dump(n);
        data_path_ = yaml["data_path"].as<std::string>();
        lio_yaml_ = yaml["lio_yaml"].as<std::string>();
    } catch (...) {
        LOG(ERROR) << "failed to parse yaml";
        return false;
    }

    system("rm -rf ../data/ch9/*.pcd");
    system("rm -rf ../data/ch9/keyframes.txt");

    LioIEKF::Options options;
    options.with_ui_ = true;  // 跑建图不需要打开前端UI
    lio_ = std::make_shared<LioIEKF>(options);
    lio_->Init(lio_yaml_);
    return true;
}

void Frontend::Run() {
    std::string txt_path = data_path_ + "sensor_data/data_stamp.csv"; 

    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
    ////////////////
    std::setprecision(10);

    sad::KaistCsvIO io(data_path_, txt_path);
    io.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
        sad::GNSS gnss_convert = gnss;
        if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
            return false;
        }
        gnss_.emplace(gnss.unix_time_, std::shared_ptr<sad::GNSS>(new sad::GNSS(gnss_convert)));
        
        LOG(INFO) << "gps numbers ......... : " << gnss_.size();
        LOG(INFO) << "gnss_convert.translation ========== : " << gnss_convert.utm_pose_.translation()[0] << " , " << gnss_convert.utm_pose_.translation()[1] << " , "  << gnss_convert.utm_pose_.translation()[2];
        return true;     
    })
    .SetSimplePointCloudProcessFunc([&](const pcl::PointCloud<SimplePointType>::Ptr& cloud_ptr) -> bool  {
        return true;
    })
    .Go();
    io.CleanProcessFunc();  // 不再需要处理RTK
    RemoveMapOrigin();
    LOG(INFO) << "gps process done.";

    sad::KaistCsvIO file_io(data_path_, txt_path);
    file_io.SetIMUProcessFunc([&](const sad::IMU& imu) {
          lio_->IMUCallBack(std::shared_ptr<sad::IMU>(new sad::IMU(imu)));                  
          usleep(1e3);
          return true;
    })         
        .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
        //LOG(INFO) << "gnss.translation ========== : " << gnss.utm_pose_.translation();

    })
        .SetOdomProcessFunc([&](const sad::Odom& odom) {
          lio_->OdomCallBack(std::shared_ptr<sad::Odom>(new sad::Odom(odom)));
          return true;
    })
        .SetSimplePointCloudProcessFunc([&](const pcl::PointCloud<SimplePointType>::Ptr& cloud_ptr) -> bool  {
           lio_->PCLCallBack(cloud_ptr); 
           ExtractKeyFrame(lio_->GetCurrentState());
           return true;
    })
    .Go();

    lio_->Finish();

    // 保存运行结果
    SaveKeyframes();

    LOG(INFO) << "done.";
}

void Frontend::ExtractKeyFrame(const sad::NavStated& state) {
    if (last_kf_ == nullptr) {
        if (!lio_->GetCurrentScan()) {
            // LIO没完成初始化
            return;
        }
        // 第一个帧
        auto kf = std::make_shared<Keyframe>(state.timestamp_, kf_id_++, state.GetSE3(), lio_->GetCurrentScan());
        FindGPSPose(kf);
        kf->SaveAndUnloadScan("../data/ch9/");
        keyframes_.emplace(kf->id_, kf);
        last_kf_ = kf;
    } else {
        // 计算当前state与kf之间的相对运动阈值
        SE3 delta = last_kf_->lidar_pose_.inverse() * state.GetSE3();
        
        //LOG(INFO) << "delta.so3().matrix() ========== : " << delta.so3().matrix();
        //LOG(INFO) << "delta.translation() ========== : " << delta.translation();

        if (delta.translation().norm() > kf_dis_th_ || delta.so3().log().norm() > kf_ang_th_deg_ * math::kDEG2RAD) {
            auto kf = std::make_shared<Keyframe>(state.timestamp_, kf_id_++, state.GetSE3(), lio_->GetCurrentScan());
            FindGPSPose(kf);
            keyframes_.emplace(kf->id_, kf);
            kf->SaveAndUnloadScan("../data/ch9/");
            LOG(INFO) << "生成关键帧" << kf->id_;
            last_kf_ = kf;
        }
    }
}

void Frontend::FindGPSPose(std::shared_ptr<Keyframe> kf) {
    SE3 pose;
    GNSSPtr match;

    //LOG(INFO) << "kf->timestamp_ ========== : " << kf->timestamp_;

    //if (math::PoseInterp<GNSSPtr>(
    //        kf->timestamp_, gnss_, [](const GNSSPtr& gnss) -> SE3 { return gnss->utm_pose_; }, pose, match)) {
    if (this->GnssPoseInterp(kf->timestamp_,gnss_,pose,match)) {
        kf->rtk_pose_ = pose;
        kf->rtk_valid_ = true;
    } else {
        kf->rtk_valid_ = false;
    }
}

void Frontend::SaveKeyframes() {
    std::ofstream fout("../data/ch9/keyframes.txt");
    for (auto& kfp : keyframes_) {
        kfp.second->Save(fout);
    }
    fout.close();
}

void Frontend::RemoveMapOrigin() {
    if (gnss_.empty()) {
        return;
    }

    bool origin_set = false;
    for (auto& p : gnss_) {
        if (p.second->status_ == GpsStatusType::GNSS_FIXED_SOLUTION) {
            map_origin_ = p.second->utm_pose_.translation();
            origin_set = true;

            LOG(INFO) << "map origin is set to " << map_origin_.transpose();

            auto yaml = YAML::LoadFile(config_yaml_);
            std::vector<double> ori{map_origin_[0], map_origin_[1], map_origin_[2]};
            yaml["origin"] = ori;

            std::ofstream fout(config_yaml_);
            fout << yaml;
            break;
        }
    }

    if (origin_set) {
        LOG(INFO) << "removing origin from rtk";
        for (auto& p : gnss_) {
            p.second->utm_pose_.translation() -= map_origin_;
        }
    }
}

bool Frontend::GnssPoseInterp(double query_time, const std::map<double, GNSSPtr>& data,
                SE3& result, GNSSPtr& best_match) {

    if (data.empty()) {
        LOG(INFO) << "data is empty";
        return false;
    }

    if (query_time > data.rbegin()->first) {
        LOG(INFO) << "query time is later than last, " << std::setprecision(18) << ", query: " << query_time
                  << ", end time: " << data.rbegin()->first;

        return false;
    }

    auto match_iter = data.begin();
    for (auto iter = data.begin(); iter != data.end(); ++iter) {
        auto next_iter = iter;
        next_iter++;

        if (iter->first < query_time && next_iter->first >= query_time) {
            match_iter = iter;
            //LOG(INFO) << "iter->first ========== : " << iter->first;
            //LOG(INFO) << "query_time ========== : " << query_time;
            break;
        }
    }

    auto match_iter_n = match_iter;
    match_iter_n++;
    assert(match_iter_n != data.end());

    double dt = match_iter_n->first - match_iter->first;
    double s = (query_time - match_iter->first) / dt;  // s=0 时为第一帧，s=1时为next

    //LOG(INFO) << "dt ========== : " << dt;
    //LOG(INFO) << "s ========== : " << s;

    SE3 pose_first = match_iter->second->utm_pose_;
    SE3 pose_next = match_iter_n->second->utm_pose_;

    result = {pose_first.unit_quaternion().slerp(s, pose_next.unit_quaternion()),
              pose_first.translation() * (1 - s) + pose_next.translation() * s};
    
    //LOG(INFO) << "match_iter.so3().matrix() ========== : " << match_iter->second->utm_pose_.so3().matrix();
    //LOG(INFO) << "match_iter_n.so3().matrix() ========== : " << match_iter_n->second->utm_pose_.so3().matrix();
    //LOG(INFO) << "match_iter.translation ========== : " << match_iter->second->utm_pose_.translation();
    //LOG(INFO) << "match_iter_n.translation ========== : " << match_iter_n->second->utm_pose_.translation();

    //LOG(INFO) << "pose_first.so3().matrix() ========== : " << pose_first.so3().matrix();
    //LOG(INFO) << "pose_first.translation() ========== : " << pose_first.translation();
    //LOG(INFO) << "pose_next.so3().matrix() ========== : " << pose_next.so3().matrix();
    //LOG(INFO) << "pose_next.translation() ========== : " << pose_next.translation();
    //LOG(INFO) << "result.so3().matrix() ========== : " << result.so3().matrix();
    //LOG(INFO) << "result.translation() ========== : " << result.translation();

    best_match = s < 0.5 ? match_iter->second : match_iter_n->second;

    return true;  
}

}  // namespace sad